%given an initial position close to a halo orbit this program
%applies a Newton's method to find the orbit (without a stopping time).

%clear the workspace
clear;
%output long
format long;



%--------------------------------------------------------------------------
%----------------------NOTES-----------------------------------------------
%--------------------------------------------------------------------------

%NAME:  EarthSunHaloOrbit_NewtonMethod.m

%DEPENDANCIES: The program calls 'CRTBP.m', 'librationPoints.m', 
%'stateTransCRTBP.m'and 'jacobiConst.m'.  The program 'stateTransCRTBP
%calls 'sysSolveCRTBP', which calls 'G_CRTBP.m'.

%These must be in the MatLab search path in order for the present
%program to run.

%USE: Once the program and it's dependencies are in the search path simply
%type:
%                  EarthSunHaloOrbit_NewtonMethod
%
%from the matlab command prompt.

%The premis of this program is that we are given initial conditions which
%are in metric units and which are supposed to be "close" to a sun/earth
%halo orbit.  We convert these to dimensionless initial conditions and 
%proceede.


%OUTPUT: The output of the program is two plots.  The forst plot is an 
%planar ("overhead") view of the earth sun system and the halo orbit.  The 
%second plot is the same but is three dimensional (even thought the initial
%view is still overhead).  In both plots the initial guess orbit is shown
%in black and the orbit to which the Newton Method converges is shown in 
%blue.  If the program works correctly the blue orbit is a halo orbit.  In 
%both plots the user has to zoom in to see the halo orbit which is about 
%sun/earth L1.  (The initial view contains the sun, earth, and L1-L5).


%PURPOSE: Given an initial condition in metric units which is close to a
%sun/earth halo orbit and a good initial guess for it's period, the program
%uses a Newton Method to find an actual halo orbit.  At present the Newton
%Method is not controlled by a while loop, but a for loop.  The number of 
%iterations of the for loop were selected after some experimentation.  A
%better implementation would have the Newton loop run until some desired 
%tolerance is met and then stop.  This is in fact what happens (the 
%tolerance is roughly 10^-9) but the number of iterations has been hard
%wired.  For different initial data, a different number of times might
%be needed.


%----------------------DATA------------------------------------------------

%parameters

%Gravational constant
G=1;

%We are given data for the Sun/Earth/Moon system where the sun is the
%primary, and the earth and moon are combined into one body at their center
%of mass.  The earth moon system is refered to as the earth.  Then

GM_sun=1.327*10^(11);
GM_earth=4.053*10^(5);

%The definition of mu gives
mu=GM_earth/(GM_sun+GM_earth)

m1=1-mu;
m2=mu;

au=1.496*10^8;

%initial position guess

%convert physical units to the scaled units of the CRTBP
%The physical coordinates are given w.r.t. the earth, so 
%a shift is also needed.
r0 = [1-mu; 0; 0]+(1/au)*[-1200000; 0; -280000]
%The time units in the CRTBP are not seconds, but the 
%physical units were given in km/sec.  So an additional const
%is needed for the velocity
v0 = (1/au)*(60*60*24*365.25)/(2*pi)*[0; -0.350; 0]

%v0=(1/au)*(60*60*24*365.25)/(2*pi)*[0; -0.3275035; 0]

x0 = [r0; v0]

%initial time guess
t_initial=1.45;
%t_initial=1.537


%Compute the Jacobi constant for the given initial conditions
C = jacobiConst(r0,v0,mu)

%---------Libration Points-------------------------------------------------


%Once 'G' and 'mu' are defined we can compute the location of the libration
%points and their Jacobi Integrals.  These will be handy when we set the
%value of the Jacobi Integral for the simulation below.

    %AXULARY CONSTANTS:
    %Compute the location of the five libration points as points in three
    %space.  These can be used to set the value of the Jacobi Constant.
    [L1, L2, L3, L4, L5]=librationPoints(mu);

    %Output location to the screen (if you want);
    outL1=L1;
    outL2=L2;
    outL3=L3;
    outL4=L4;
    outL5=L5;
    %Now compute the Jacobi Energy at each collinear libration point

        %The velocity at any equilibrium is zero.
        v_equil=[0; 0; 0];

    %Compute the energy at each libration point
    CL3=jacobiConst(L3, v_equil, mu);
    CL1=jacobiConst(L1, v_equil, mu);
    CL2=jacobiConst(L2, v_equil, mu);
    CL4=jacobiConst(L4, v_equil, mu);
    CL5=CL4 %so no extra computation is needed


%--------------------------------------------------------------------------    
%Aux Matrices
%--------------------------------------------------------------------------
O=zeros(3);
I=eye(3);
K=[0, 2, 0;
  -2, 0, 0;
   0, 0, 0];


%--------------------------------------------------------------------------
%----------------------Newton Method; find orbits--------------------------
%--------------------------------------------------------------------------

%initial guess
x_n=x0;
tau_n=t_initial
tau=tau_n;

K=14;
for i = 1:K
    out_i=i
    %We need some infromation from the 'reference trajectory'.  
        t0=0;
        tf=tau_n;
        numSteps=2000;
        tspan=linspace(0, tf, numSteps);
        %numerical integration
        y0=x_n';                                          %inital condition
        options=odeset('RelTol',2.5e-14,'AbsTol',1e-22);    %set tolerences
        [t,x_ref]=ode113('CRTBP',tspan,y0,options,[],G,mu);
    %Start computing the differential of the Newton condition vector
    %Need the STM for nth guess at nth time (tau_n is n+1 th time)    
        Phi=stateTransCRTBP(t0, tf, x_n, mu);
        %some of the terms depend on the vector field
        f_x=vectorField_CRTBP(x_ref(numSteps,:),G,mu);
        %Construct the needed differential
        a11=Phi(4,3);
        a12=Phi(4,5);
        a21=Phi(6,3);
        a22=Phi(6,5);
        a31=Phi(2,3);
        a32=Phi(2,5);
        %the differential of the constraint vector
        DF=[a11, a12, f_x(4);
            a21, a22, f_x(6);
            a31, a32, f_x(2)];
        %Invert
        D=inv(DF);
    %Compute next (or final/target) initial conditions    
     %compute next step
  Term_1 = [x_n(3); x_n(5); tau_n];
  Term_2 = -D*[x_ref(numSteps, 4); x_ref(numSteps, 6); x_ref(numSteps, 2)];
       x_star= Term_1 + Term_2;
    %set up x_n for next itteration or output
    %Put it all into x_n (which is really x_(n+1) as it occurs at the end
    %if the loop
        x_n=[r0(1);
            0;
            x_star(1);
            0;
            x_star(2);
            0];
        tau_n=x_star(3);
end    

outX=x_n
outTime=tau_n

CTargetOrbit=jacobiConst(x_n(1:3), x_n(4:6), mu)
deltaInitialEnergy=abs(C-CTargetOrbit)


%-------------------------HALO ORBIT---------------------------------------

%Now compute the whole orbit using symmetry
t0=0;
haloPeriod=2*tau_n
numSteps=2000;
tspan=linspace(0,haloPeriod,numSteps);
%numerical integration
y0=x_n';                                             %inital condition
options=odeset('RelTol',1e-12,'AbsTol',1e-12);     %set tolerences
[t,x_halo]=ode113('CRTBP',tspan,y0,options,[],G,mu);


%Some analysis of the orbit;

%check the order of magnitude to which the orbit is periodic
periodicityCheck=norm(x_halo(1,1:3)-x_halo(numSteps,1:3))

%Compute the monodromy matrix for the periodic orbit
Phi=stateTransCRTBP(t0, haloPeriod, x_n, mu);

%determinant of Phi;
testDetPhi=det(Phi)

%Get eigenvalues and eigenvectors
[Phi_eigVecs, Phi_eigValOnDiag]=eigs(Phi);

PhiEigenValues=diag(Phi_eigValOnDiag)



%Simulate the original conditions
tf=2*tf;
numSteps=2000;
tspan=linspace(0,tf,numSteps);
%numerical integration
y0=x0';                                             %inital condition
options=odeset('RelTol',1e-12,'AbsTol',1e-22);     %set tolerences
[t,x]=ode113('CRTBP',tspan,y0,options,[],G,mu);


%----------------------------2d plotting-----------------------------------

figure
hold on
%Contour Plot of the Energy Surface
   [X,Y]=meshgrid(-1.1:0.002:1.2);
    r1=((X + mu).^2+Y.^2).^(1/2);
    r2=((X + mu-1).^2+Y.^2).^(1/2);
    Z=2*[(1/2)*(X.^2 + Y.^2)+(1-mu)./r1+mu./r2];
    contour(X,Y,Z,[CTargetOrbit, CTargetOrbit],'r')


%plot the initial trajectory
plot(x(:,1), x(:,2), 'k')    
    
%plot the halo orbit
plot(x_halo(:,1), x_halo(:,2),'b')

%plot the libration points
plot(L3, 0, 'ko', L1, 0, 'ko', L2, 0, 'ko')
plot(L4(1,1), L4(2,1),'ko',  L5(1,1), L5(2,1),'ko')

%plot the planets (primaries)
plot(-mu,0, 'r*', 1-mu, 0, 'g*')


%-------3d plotting--------------------------------------------------------


figure
hold on

%plot the libration points
plot3(L3, 0, 0, 'ko', L1, 0, 0, 'ko', L2, 0, 0, 'ko')
plot3(L4(1,1), L4(2,1), 0,'ko',  L5(1,1), L5(2,1), 0,'ko')

%plot the planets (primaries)
plot3(-mu,0, 0, 'r*', 1-mu, 0, 0,'g*')


%plot the initial trajectory
plot3(x(:,1), x(:,2), x(:,3), 'k')

%TEST: Plot the stopping point of x_halo
%plot3(x_halo(1,1), x_halo(1,2), x_halo(1,3), 'r*')
%plot3(x_halo(numSteps,1), x_halo(numSteps,2), x_halo(numSteps,3), 'r*')

%plot the halo trajectory
plot3(x_halo(:,1), x_halo(:,2), x_halo(:,3), 'b')

axis([-1.2 1.2 -1.2 1.2 -0.006 0.006])



